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(54) METHOD AND SYSTEM FOR RECOGNIZING THREE-DIMENSIONAL POSITION AND ATTITUDE ON THE BASIS OF SENSE 
OF SIGHT 

(5 7) Abstract: 

PURPOSE: To robustly recognize the position and attitude at a high speed and 
with high accuracy by diagonally sensing a plane marker by a sensing means and 
performing a simple image processing. 

CONSTITUTION: A large circular part 7 and a small circular part 8 are displayed 
on a plane marker 6. A three-dimensional recognision system 9 consists of an 
image processing part 10, a position/attitude calculation part 11, etc. The marker 
6 is sensed diagonally by a CCD camera 5, and the center of gravity of the elliptic 
part 7 is obtained based on the image data, then, when it is out of the central 
position of a sensing screen, a central position correction calculating part 13 
calculates the rotational angle for the camera and rotates the camera 5. Further, 
an elliptic approximation calculation is carried out by using a minimum square 
estimation in order to obtain its parameter and calculate the position and attitude 
of the camera 5, and then a coordinate conversion matrix is calculated based on 
the obtained result. Thus, the image processing for the marker 6 can be performed 
simply and at the same time its position and attitude can be recognized robustly, 
at a high speed and with high accuracy. 
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&{c#tj£-r SfSIJIIiE±-c©J^tll.£<!i: t*5S±«:tt-S L so 
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So 

[0 04 1 ] CftW. CCD^^7 5©7tlil*Si£«Cfl| 
^SM^e»v-*6©*ffi*m : 'fc{4g{CA e c? > |i 1 JggJ 
BffliCC fctt S «RflJtt©fi-l>ftg<!: ARSIS 7 ©m&ffi 

g<t*s-rfts*^-c$>«D. ^-r-sfifg^^©isi«B#K 

fcc^rti. C©|e^^f#S/cW^j:<-r-<< . ffiiE© 

fts^nsgL-r^So 

[0 042] ( c ) ^-C^iSWSteSXT- ^(S4) 
. X f -j rf S 4 CC mts t . tf -C^g*lJSBP 1 7 Kte 
OT. t>$lt#L,/cARa57©ct>^<agS#Pa (p 

x, P y) ±tawmm<D*p&&mm&o (o, o> <t© 
mvfe&Z9twux<p.cf$mMDptLx&m.L> -e© 

tfMMB&St D p ©«^S|fiM«i 5 &#«S#>£ 
[0 04 3] Cft«. »i5U/c-7-*6©ARgP7^J| 

KBi®±K:*5t^r-e©^{4gP a#JfSJiiraff©cW>fii 

gO©£&tC#ftUTC>S*>S*>£¥lJ5rrS <b<DX\ 
TNOj ifTOrSftftitB^tcB^^y^SStcJi*. f& 
jzETSJ: 5«C. ARSIS7©tfrl>(igP a**£HfflB©(p 
<KSO tcaft <<fc^KCCD*y75 ©sgf£#*(H3p $ 

[0 04 4 ] ( d > tpsbGLmmjEjmx? ^(ss. 

S 6 ) 

SP13tcJ:0. ±$£©v-#6©AR8in©^i>{i§P 
a (px, py)Olgf-f*f>, #©cfc5(CLTC 
C D t> M 7 5 ©figjE r - ^ *f t#T -5 J: 5 (c * S . 
[0 04 5] l>$, CCD^^55(CJ:0«SJ3ftTl» 

€.1®®*^ 05 (a) K.m^x^tmmi<cs>i>m^m 
t,ct->xmw?&. c©j«^ -^-^eoARgp?©* 

-CMPa (px, py) it, igU®ffi©ttM>{igO© 
*± <*1*R> K#fiOr*lO. MUlliffiiCCD* 
>75©a.^Vitt. IUH (b) {C^T<fc ^ feiBSM^ 
i^->ri,iSo 

[0 04 6] CCX\ CCD^^7 5©*Wl^*Y 

«i. *«tcaii[aiifrt©±**ztt. ^Mtcsgtncisn 
©*i*^*i^t>r*^iaj^xwi-rs<!:. ccd*^ 

5 5 ©RMt©fit««r-t©* S©R^C«f#l/^C/&56, 0 

±{C*iWS AR95 7 ©4>^v&S Pa* Xtt^r^tc p x lie 

BRSer«KiBiB±«:*jW*^R»7©«f ^BP a 

RgP7©*^SP a^jgfjiijoiJbCDcfi^fiigOCCSKj 
3-tiSCi*5-C*So 

[0047] -ec-c. ±a2©(siis^s!j»(cfc(-ts. z 

^©HSteftSfc i^XWil K> ©HiKftE^-eft^ft 
ffliEftgDl, D2 ite^T, JWTCJr^CCL-rCCD 
-5 5 ©31£W*ARSP7 ©i^^fcigP{C[i]^Sj)3# 



(7) !8P3¥7 -98 2 0 8 

11 12 
•5>/c*©^^f?iJT o/p%3j<«>, X7-y7'S6Kfct,> *(5iCft*j<i > tanGl*jJ:0* tanG2«> T(C^T 
t, dsj?» hA>F l©W(a«SBK:jWaifl|-«»iL/r#iL 5£(1). (2)Oj:^i:ifC«!:mS. t 
&<fcM<:&5„ anD lfcitK tanD2«. Tfc^Vt5£ (3) , (4>© 

[0 04 8 ] T&fr%. S-T. CCD*y^5tCj:S}l J:5(C«fCi*5tS^e>, 5£ ( 1 ) . (2)^6^ 

uiisf**-rfii*isj*j<t^fijWi6itc»-r5*^(aso*s m f ©fii£-eft-eft5j?#-c5£ ( 3 > . ( 4 > KttA-r & 

6©BiftGlteJ:<>*G2i, CCD*^^5CD^iaS <!:, tanD 1 toJzU tanD 2 « v i$ ( 5 ) , (6)<D=fc 

±«C fcW P H *5<£ O'SiL^^-ffi PVB 5 KilT C <!: ^"C # & J: 5 tcfc £ . 

Wa<?-Z-C3bZ>frh. ffliEftlgD l*icfcJ>*D2*feiT [0 05 0 ] 
©<fc5tet-T3fc&£C<t#s-C , i*£, > 

[0049] CCD*^55 <t JSKiEffi£©IISI©8E!8£ *10 
tanGl=|y- — (1) 

PV 

tanG2=y|- (2) 

tanDl (3) 

tanD2=^- (4) 

t«nDl- 2p »p t || anG1 (5) 

tanD2= ^-^ nG2 (6) 

[0 05 1 ] iC^t, C©ct5tCf|IE#gD 1 fc<ty KiJ J 5 S(Dffi&1ifa*m§WtttA&C&llr}fct%lC 
D2-CEIlK-r-5ia^(C. HBSK«. m*.&. Wto^CZW CCD*>75CD±^hJKJ, 5£ (7) ©J:MC& 
fflK) ICC CD* J 55 <Dmtl%:miEft&D ltctf\affi.£ -5. C D D2M ^ 5 ©Sua'** hJU£:*:RSP7 © 

5A"P1S|57©itM>tegPa«, @eOfc«©JMJHiffl_k 3C ( 8 ) ©<fc 5 tn?.©!^^ h;KD 

£9 i>jsWig&&*£©-e. m&Ltc&<Dm&WM±.lc JWI*tf 5 c£&c<fcDf#6ft&-<i' hMiaS; (9 ) ©J: 
i9^Ufci!r©^aMPa<Dy|^«fiipy*jffiiE-r€» 30 SKft-S. 

[0 0 5 3 ] 

[005 2] C®<fc9ttj5&Mtt'£&. CCD* [&2] 

(0, - tanD2, 1 ) — (7) 

( tanDl, 1. tanD2) (8) 

( 1 + tan 2 D2, - tanDl, - tanDl* tanD2) ( 9) 



[0 05 4] b/a)ioT, fcLhSt (7) fc^L (9) © FUTo/pli, S(10) ©J;^#f>n&o 
J:^(Cff e>ftfc&-<^ hiU*>6, CCD*>754AR 40 [0 05 5 ] 
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To/p 



1 ± tan D2 
AB 

- tanDl 
AB 

- tanDl tanD2 
AB 



(8) 

tanDl 



_L 
A 

tanD2 
A 



0 0 
- t anD2 n 



B 



B 



0 
1 
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c io) 



fc*fLA= V I + tan 2 Dl+ tan 2 D2 



1 + tan D2 



[0 05 6 ] -e-L/T. C©i^fcltCCD*^75CD 

tf^ffiSfeTRCfcofctSCC. ^.T-^yS7CC^tf-r 20 

[0 0 5 7] ( e ) fi^g^^ltSLMaXf- y 7' ( S 7 ~ 
S 1 0) 

C©tKS8rB. V-*6©^cP3^P7(Dcfi-CWigPaiL 

T3£«>fcA0&g#. JS12Ba©<fr[>ffittotc— Sfcur 
l>Stt«ttt-9'ri»SC!)'C. *RgB7©SE©tp4>&gP 
a # ««— ifc L r c > & i ^ ft h r «t fc^-r J; 5 &{ig 
^©ItlTMJlSr^T "T S J: 9 fc * . 

« Ri£{Hft#g|5 1 8 K r . -7- ft 6 © =fV> >? >\>W&?- 30 
*(C»~5t,>T:*Rg|5 7©i "j ^:r-££fflLHL, 

[oo59]ia7«i^ s> f 8 - ^ottuaaHieiSiw 
-r 6fcat>©k©T\ i8tt-e©7P-ft-h^t 

[0 06 0] $-r. f*-^^o/ca*KD*s*ia^-r.2» 

•c^&fr&frzmm-rz (xf-^T2) t rNoj t 
mm £ ntc t z tea. =7 j >im~r* & * t?©ih . 

Xf^TS^it. Xf-^T lttlr»tsT3£*9£ 
SS5-f>»7L.tXr^'T3 7 r Y 
ESJ £*iJtBrS*lA:<!:#ic«. ^f-^-7'T4*ig-C^^ 
•;^T5iCjtt^ j£3r^>*i"r^»ft:tt»»3-tf. 50 



[0 0 6 1 ] *Ur. Xf 5>^T2T r Y E S J Lmm 

3n/ci#ic«, ^f--y^T6ccii*. ^-^©^tiPi 

7tii. Xf-^T4T TYESJ tm&ihX^^ 
[0 06 2] C©J:-5K;L-C. «R«tt*tt-*-*RS$7 

Htf L.Tt»R©^|iS;?rf#S„ C©i*fflC^S 

[0 06 3 ] sr. c©J:5KLT#a&?>ttft:;*:RSB7 
R©S#^afe^^Mrt^b**JbSirttc. -e© 
ttfa©^^^^-^ 

[0 064] HQttCCDfcy 55©«R«fiH_h«CfiSJ 
$ft/cv-#6©B5«Sr^LT*J*K «RJ8tR*fc-r* 
RSB 7 ©tp^fiSP a ttlgS5Hffi©#'l>{igCKi:— iSc 0 

ri»*tt«r*s. -eur. *n«J7K:*tifrrs«pij& 

tttt-e©fi*fc a *J «fc CtiSN b #IB12BOT5-h©ffi««K:*t 
l,T *^(5gO £0*tetM> £ 0 T ftS a /cWZE^ 
tcO!H6S*ifc««i!&on»&. C©tt»r. ARSB7 
©Sf A a HE mitimwm iW«rt(Ci¥4Lrt,^i 
#^.€.C<t*5^^.5©-C. ja^lBli±«:*JWiS«|a© 
^fiSB, -?-#6©ffi#«:MG&&< > CCD*y75i 
v-*6©^;RgP7 U-COffiatdKieCfc^-^ift 
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[0 06 5 ]f It, ^f-^S Qicmtft. {4gtf-gt 

T t, > &/JnR3|S8 ©cb-lXiiS P b &nk? feU?- 5 &C»-3 
l>t . *MIJIBffi±«:iJW«*P«R 7 fc<ti>-^pqg|5 8 ©«• 

Rg|5 7 ©gflhJT* a*>6CCD*>55 ©«.£.*» 6*R * 
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2«p«tan- (A/2) 



* 8B7 ©cNXagP a $T'©ffi8t d 

[006 6] tto^, ^-#6©*Rffl7©SlfiK©iB 

&a©Bftj&**>*>&©-C. ^ < 1 1 ) ©iStClz-C. 

[0067] 
UK4] 

(11) 



D : v-^^PJ«B<D®S^m 
p ^fiUOSS&MTi-C'©^ 
o> : «J&IB«1©« (81) 3ffir*& 
A : CCD*^7®a («) 9ft 



[0068] awe. xf? i otcjtt*. mmtmu 

2 i fctei^r, jy±©.£5&coT^&fc:gW\f1£a, @ 20 
*Wj*b*J«tCJfJ8aid©«!{ca-^Sf, WT©<t^{CL 
T. CCD*>75 0j(SI£ill07-*6 0ltOiEc 

■rfflftftssstw-rstfttc. f» tn/tiige £sueE 
gP8©#^i:>aapa, Pbra&«§.&ifij*d:j@ttb£© 

&-T<«£#S<S i»6fl|a £^RSB8©4m>j5P b i 

©& -r mm<om # a k * * *© s . 

[0 0 6 9 ] m 1 Ott, 19©j: O^JjUJfiiffl^^n 
TO SttStO , CCD*>75 ©II * V t v- f} 6 © 
ia^M^^-riar-* 0 . ^-#6©ffl*5Jg|£iiiti-e& 30 

* 6 ©7CR3P7 (Dip^iim P a S ©BftyC&g 

u ^RaJ7*ss^ffls<b«wsa5^©^*si§i9tc*j 

[0 07 0] 2t\ 0t£K*$l>T, CCD*>75Ci 
*V £*RgR7 ©tb-lXAH P a Zm&m&~? *?)*«£ 



v-*6©M£^-rM*^JS0<CO^T**^> o C©fli 

^ftffiew:. m 1 1 cc^tj^k:. iiootiiss; 

ff R©fif A a ft*, m/c t * ©v- * 6 ©ffi £CCD 
£©&-fM#ftffitC^Ll.>*>6, iTj£©5£ ( 1 

1 ) ©<£ ^ ttfms n/c^snd £ & , muntm 

[007 1 ]01 lKfc^r, CCDW7 5©tH£V 
£ -7-t, 6 ©ARSP 7 0±«£T«£ £-£n-en*§A./t 
£#(c&15ffiS£^-2>*£t lfcJ:l>'t2£t5i, 
SUE S ©Hj£0 £ * t 1 , it2£ £Zft?1nt&&®. 
»rl, r 2©fi3^*£:£©6<!:, 5$ ( 1 2) , (1 
3) ©J: 5 Kite 3. -ect:. 1 fcjctf r 2 ©*D 

«. «R©JSftN-&b«:^L< &£©-?. C*i6©M{& 
*>6. s£ ( 1 4) stctt*. LfcAior. 

S (14) £ sin6 tC~?l*X:M< £, 2#;frg5S;£&-5 
*>6. 5S (1 5) ©J:^K*©»*»*c£*i-ras. 
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rl 



d + r * cos 9 



Cio) 

r-sinfi ( 12 ) 
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r2 



d - r • co s 9 



r ■ 8 i n 0 



(13) 



fcfc'Lr- §- (a :fi*HT&) 



rl+ r2 



s i n 6 = 



2 ; d T-sinfl _ 



.2 2 i 

d - r -cos 



9 



= b 



2«d 



-V 



.4 . ,2.2, 2 .2 

d — 4 ! d «b + a 'b 



a * b 
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<fct). *B»7©IEMtt^CtoaPa*jR«>£C<b*i-C 

n-5^pqgp7©*^(igis^©AngiJ7 ©*<i>&ga* 

#r 1 £ r 2©S3*sS&-5Ci*>6^-r-5feCDr. It 
Atrtttc ti &S<bttS P e tc*f L r . 5ll£© * 

r2- rl 



d - sin * cos fl 
2 -**-cos 2 9 



r 2 <b©M*2-C*!lofc^A/cWfi<C^^6^«)5 
mc&tt£Z>. */c. COWHAtt. CCD*>750 
20 6 A^a^**!. C©WStt4>ite<tt 

[0 07 4] 
[&6] 



(16) 



a 



[ 0 0 7 5 ] g«a t/jNRgP8 ©tpAjgp b £© 30 

si 2«. Hi l ilsi^KcL-rSiJffiS^W 

ROfilia^flSjJ&ifefifcWffiiar*"). HI 3«CCD 
ij ji "7 5 ©ta* t v - # 6 t (Dfom.Mfc'&iI.W&ilitTr; L 
?c4>©-C*>^ 0 cnewH*. jSuWSIKS©^— #6© 
^nSP7©*^*P a t/hRg|58©4J^P b 
«iStt*s*PWIJ7©ja*l«i^5|-rS^*^Lrfe») . 
& u a KSJKffl S ±tc 3 *l fc.<±> u ©teg ?:7nL/"Cl^ 

[0 0 7 6 ] S»MS±(C*Jt^r, £ua*fiS#ali 40 



«fct«®l*bSr©re«u v*sJ;0*uh©S3^t7£«. A 
R8B7©¥Sr (=a/2> . SgfSd. <*> 

^ii^ti, -en-en^(i7) , (18)©^ 
tea*. mmomzPim.eimmuhid^vu v<Dm& 

*>6>. tan/3 iLr^-r£> 5£ ( 1 7) teitf ( 1 8) 
*>65S: (19) ©J^K^^-ft^ Ucdiot, tflg-ft 
ffi*«. tt^Mefci^^i^ffl^TS (2 0) ©J; 

[0077] 
[«73 
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uh= r -cos *• sine- d+ r . cos £. C os 6 

d 



u v = r • s i n $ 



d+fcos ^ • cos 9 



20 

(17) 



C 18) 



uh sinS 
tan0- uv - tan ^ 



( 19) 



t an 0 = 



sing 
tan $ 



[0 07 8 ] m±<r>m^. 6©{igsSi*£Srrfc 

[0079] ( f ) wmmy mmmmz r » < s 

11. S 1 2) 

MARK 

= TRS (0. d. 0) -ROT (y. a) -ROT (x. 9) -ROT ( z. 0) (21 ) 



(20) 

* &&&%mir&xmmm$m<ommg.m : T mm a rk* 

5$ (2 1) ©cfc'5&CJj<#S.=t i 5(C&S <> C 
^SitfT?IJMARKt;fc, S(22) tC^Tcfc i 5%4tf4?iJ 

[0 08 0] 
[«E8] 



MARK 





yi 


Zl 




x 2 


Y2 


z 2 


Py 


x 3 


Y3 


Z3 


Pz 


0 


0 


0 


1 
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Cx 1 ,x 2 ,x 3 ) : v — *&JS56x$AG>x. y, z fiK# 
( Yl- Yz> Y3 ) : -?-#&^3&y <*£>x, y, z Htd 
(zi,z 2 .z 3 ) : x, y . z 

C^x.Py.Pz) : v-rt^^!6J0§Ci5<&x, y. z 

[ 0 0 8 1 ] Sfc, ±a<Z>ffi**8ttfWMA RK<Z>&|£ [0 0 8 2 ] 

»*^Tffi«SOftffWTRS. ROTtt, *S(2 3 ) 40 [89] 
(2 6) 0cfc5{c5g»S4rr<r>*. 
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TRS (x. y. z ) 
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(x. y, z> \z¥f7&m?% 

T**t5U (4x4) 



TRS (x, y, z ) = 



1 

0 
0 
0 



0 

1 

0 
0 



0 
0 

1 

0 



X 

y 

z 
1 
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ROT (r. 



ROT (x. a>) = 



TXfim (4x4) 



10 0 0 

0 cos Oil -s i n a 0 
0 sinoi cosoi 0 



L 0 



ROT (y.w) = 



ROT ( z , oj) = 



cos b) 0 s i n (o 

0 10 

-sineo 0 coscu 

0 0 0 

cos d) -s inw 0 

sino) cosw q 



0 
0 



0 
0 



1 
0 



1 J 

0 
0 
0 

1 

0 
0 
0 
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[0 08 3] CCDt, X?7 7"S 1 2 r-d? 
m^S52 2rt>6> ffi*^ftfT5«MARK{c»^>fcttB 

[0 08 4] C ©J; ^ fc#SQW!ftt: «fcft«. ¥W-htcjft 
^2nfcARSC7feJ:C>v|NRg|J8^6^-5>v-^6^C 
C D 2> V 7 5 «C J; *) mjjft] 6 JS K L T C C D il * v 5 

ft*. 

[00851 ^/c, ^HifeWc^tltf . £<CriHM 1 6 



gfl7©fti>{iM«:#j-L/T, A^y-^ttficgB l 9(c*50> 

[ 0 0 8 6 ] m 1 4 t£l>Vm 1 6 «#fg?)J©?jl2 £et> b 

2 3. 2 4, 2 5*ffll^c«-^-fCO(,>-C^U/cfe©-e& 

T«. *RgR7 Ktt*.TRSM#£&Tr*R§|J2 6 ZtkV 
fc4>©-C&&„ ^©^teWcfcttS^-*^ 4it*st,> 

rtt. jg«g(5*«-r/jNpqgR8K:f^^r. *rsb2 7i*gtc 

6feT'^L^R^2 8£f&tf/cfc©-C*£o S84©ji 

l>fcAH*2 6 tC*g«SR£ Or©^En«?:^f Hfe©J§ 
1§2 9*Hii3-a-SJ:'5(c^L'fc^©-C*.So 
50 [0 08 7 ]ftt. COJ;5^2&l>( / f4cr«i6 
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2 5 (D&mgmzim-r s c t #-c # & 



*2 3, 2 4, 

&©r*&„ 

[0 08 8] ±iB£-3USWt*a>T«. -7-* 
6. 23, 24, 25 4, Bifi(cl!l,&r?PW&asteJ:cHB 

[0 089]fLt, ±K«£JfcWc*t,»-Ct*. 
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* [0 09 0] 
ltS?©l&^ 

ffi&<D7'V A CD X7-yy'S7 &CTH?T 2 tl -5 , fibh 

[Bi&I^] IE8 8 - 1 08, P9 - 1 6&C»*t3ft 

<»iii*=. /mam) *#jwort>s. 
[0091] *r. £m^*«R©#«5££*-ffc<* 
io k . ( a > r svj- «t ^ a — jase^ ft 2 sa: f 
(x, y) iftje-r^^^^y-f »h. 

b, g, f, cifti. tir. COJ:9«c2%ddi«> 

9*-**«rflWi#»ww <b) cD«fc^(c#A6n-ct,» 

[0092] 
: [»10] 



f Cx.y) = x 2 + 2hxy + by 2 4- 2gx + 2f y + c = 0 - (a) 



Cn1.n2.n3, 



Cb) 



[0 09 3] C ©i # , «/]N-m^4tf 5 *|-£©f¥ffi 5K 0 ( h ) *^fc-T<fc 9 K3^£&Sj£*JBttUCm>. 
mm. J *, ^ (c ) iffl^Si, ^^y-*h. b. [0094] 
g, f, c fca&tct*. §A7y-^TM»l [ftll] 

fcisma^T "o" &ttntxai>£>&. <<j> 

N 2 2 2 

J = Z (xi +2hxiyi + byi +2gxi+ 2fyi + c) (c> 

i = l 



8 J 

8 h 

9 J 
3 b 

a j 

88 

a j 

a f 

_2J_ 
a c 



N 

4 E ri xj yj =0 
N 2 

2 Z ri yi =0 

N 

4 E ri xi -0 

i=l 
N 

4 £ ri yi =0 
i=i 

N 

2 £ ri =0 
i=l 



(d) 
(e) 
( f ) 
Cg) 
(h) 



tztiib r i = xj +2hxiyj + byi + 2gxi+2fyi+c 



[0 09 5] CiT,%5mo>'^j'-Zfr't>13:&'<i> bJl -*h. b, g, 

P^m^XVin-C^mrT^t. 3£ ( i ) ©«fc5Ccft£„ [0 09 6] 

lfc*ioT, C©iS ( i ) l^h^-'P h-rt/Pfcsfe&ai. [S12] 
SC ( j ) ©<£ -5 ioRa?>£C cn*<5^9> 



(14) 
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2£xi 2 yi 2 Zxiyi 3 2£xi 2 yi 2Exiyi 2 Exi yi 
2Zxiyi 8 Syi 4 2£xi yi 2 2Syj 8 E yi 2 
2Zxi 2 yi Sxjyi 2 2Dxl 2 2Exiyi Exi 
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